#include "StickFigure.h"

#define PI 3.14159265
#define BONE_SIZE 0.5

StickFigure::StickFigure(){
	center = Vector3f(-0.1 , 0.7 , 0);
	
	Link* head = new Link(center, Vector3f(center(0), center(1) + BONE_SIZE, center(2)));
	Link* spine = new Link(center, Vector3f(center(0) + 0.1, center(1) - 2.5*BONE_SIZE, center(2)));
	
	Vector3f leftShoulderEnd(center(0) - 1.7*BONE_SIZE, center(1), center(2));
	Link* leftShoulder = new Link(center, leftShoulderEnd);
	
	Vector3f rightShoulderEnd(center(0) + 1.7*BONE_SIZE, center(1), center(2));
	Link* rightShoulder = new Link(center, rightShoulderEnd);
	
	Vector3f leftRibEnd(center(0) - 0.8*BONE_SIZE + 0.1, center(1)- 1.5* BONE_SIZE, center(2));
	Link* leftRib = new Link(Vector3f(center(0) + 0.1, center(1) - 1.5* BONE_SIZE, center(2)), leftRibEnd);
	
	Vector3f rightRibEnd(center(0) + 0.8*BONE_SIZE + 0.1 , center(1) - 1.5* BONE_SIZE, center(2));
	Link* rightRib = new Link(Vector3f(center(0) + 0.1, center(1) - 1.5* BONE_SIZE, center(2)), rightRibEnd);

	Vector3f leftHipEnd(center(0) - 0.5*BONE_SIZE + 0.1, center(1)- 2.5* BONE_SIZE, center(2));
	Link* leftHip = new Link(Vector3f(center(0) + 0.1, center(1) - 2.5* BONE_SIZE, center(2)), leftHipEnd);
	
	Vector3f rightHipEnd(center(0) + 0.5*BONE_SIZE + 0.1 , center(1) - 2.5* BONE_SIZE, center(2));
	Link* rightHip = new Link(Vector3f(center(0) + 0.1, center(1) - 2.5* BONE_SIZE, center(2)), rightHipEnd);

	leftArm.initLink(leftShoulderEnd, Vector3f(0, 0, 1), 45*PI/32, BONE_SIZE/3);
	leftArm.addLink(Vector3f(0, 0, 1), 0, 1.5*BONE_SIZE);
	leftArm.addLink(Vector3f(0, 0, 1), 0, 1.2*BONE_SIZE);

	rightArm.initLink(rightShoulderEnd, Vector3f(0, 0, 1), -3*PI/8, BONE_SIZE/3);
	rightArm.addLink(Vector3f(0, 0, 1), -PI/24, 1.5*BONE_SIZE);
	rightArm.addLink(Vector3f(0, 0, 1), -PI/12, 1.2*BONE_SIZE);

	
	leftLeg.initLink(leftHipEnd, Vector3f(0, 0, 1), 23*PI/16, BONE_SIZE);
	leftLeg.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE);
	leftLeg.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE);

	rightLeg.initLink(rightHipEnd, Vector3f(0, 0, 1), -7*PI/16, BONE_SIZE);
	rightLeg.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE);
	rightLeg.addLink(Vector3f(0, 0, 1), 0, BONE_SIZE);

	body.push_back(head);
	body.push_back(spine);
	body.push_back(leftShoulder);
	body.push_back(rightShoulder);
	body.push_back(leftHip);
	body.push_back(rightHip);
	body.push_back(leftRib);
	body.push_back(rightRib);
	for(int i=0; i< (int) leftArm.links.size(); i++)
		body.push_back(leftArm.links[i]);

	for(int i=0; i< (int) rightArm.links.size(); i++)
		body.push_back(rightArm.links[i]);

	for(int i=0; i< (int) leftLeg.links.size(); i++)
		body.push_back(leftLeg.links[i]);

	for(int i=0; i< (int) rightLeg.links.size(); i++)
		body.push_back(rightLeg.links[i]);
	initializeFrame();
	frame = 0;
}
void StickFigure::initializeFrame(){
	Move[0] = 0;
	Move[1] = 1;
	Move[2] = 2;
	Move[3] = 3;
	Move[4] = 4;
	Move[5] = 3;
	Move[6] = 2;
	Move[7] = 1;
	Move[8] = 0;
	Move[9] = -1;
	Move[10] = -2;
	Move[11] = -3;
	Move[12] = -4;
	Move[13] = -3;
	Move[14] = -2;
	Move[15] = -1;
	
}
void StickFigure::walk(float scalar){
	Vector3f leftArmPos;
	Vector3f rightArmPos;
	Vector3f leftLegPos;
	Vector3f rightLegPos;

	
	leftArmPos = Vector3f( -0.7, -1.91, -0.2 * Move[frame]);
	rightArmPos = Vector3f( 0.69, -2.33, 0.2 * Move[frame]);
	leftLegPos = Vector3f( -0.70, -2.78, 0.2 * Move[frame]);
	rightLegPos = Vector3f( 0.77, -3.36, -0.2 * Move[frame]);
	/*
	switch(frame){
		case 0:
		case 4:
			leftArmPos = Vector3f( -0.7, -1.91, 0);
			rightArmPos = Vector3f( 0.69, -2.33, 0);
			leftLegPos = Vector3f( -0.70, -2.78, 0);
			rightLegPos = Vector3f( 0.77, -3.36, 0);
			break;
		case 1:
		case 3:
			leftArmPos  = Vector3f( -0.7, -1.91, -0.2);
			rightArmPos = Vector3f( 0.69, -2.33, 0.2);
			leftLegPos  = Vector3f( -0.70, -2.78, 0.2);
			rightLegPos = Vector3f( 0.77, -3.36, -0.2);
			break;
		case 2:
			leftArmPos  = Vector3f( -0.7, -1.91, -0.4);
			rightArmPos = Vector3f( 0.69, -2.33, 0.4);
			leftLegPos  = Vector3f( -0.70, -2.78, 0.4);
			rightLegPos = Vector3f( 0.77, -3.36, -0.4);
			break;
		case 5:	
		case 7:
			leftArmPos  = Vector3f( -0.7, -1.91, 0.2);
			rightArmPos = Vector3f( 0.69, -2.33, -0.2);
			leftLegPos  = Vector3f( -0.70, -2.78, -0.2);
			rightLegPos = Vector3f( 0.77, -3.36, 0.2);
			break;	
		case 6:			
			leftArmPos  = Vector3f( -0.7, -1.91, 0.4);
			rightArmPos = Vector3f( 0.69, -2.33, -0.4);
			leftLegPos  = Vector3f( -0.70, -2.78, -0.4);
			rightLegPos = Vector3f( 0.77, -3.36, 0.4);
			break;	
	}
	*/
	leftArm.calculate(leftArmPos/scalar);
	
	rightArm.calculate(rightArmPos/scalar);
	
	leftLeg.calculate(leftLegPos/scalar);
	
	rightLeg.calculate(rightLegPos/scalar);
	
	frame++;

	frame = frame%16;
}
void StickFigure::display(){
	
	for(int i=0; i< (int) body.size(); i++)
		body[i]->draw();
}

void StickFigure::test(Vector3f goal){
	rightLeg.calculate(goal);
	
	//leftArm.calculate(goal);
}

void StickFigure::associateLinks(triple* t){
	float min = (t->myVertex - body[0]->getMid()).norm();
	Link* min_Link = body[0];
	for(int i =1; i< (int) body.size(); i++){
		float dist =  (t->myVertex - body[i]->getMid()).norm();
		if(dist< min){
			min = dist;
			min_Link = body[i];

		}
	}
	t->myColor(0) = min_Link->r;
	t->myColor(1) = min_Link->g;
	t->myColor(2) = min_Link->b;

	t->myVertexLocal = min_Link->getLocal(t->myVertex);
	min_Link->vertices.push_back(t);
}
